#include <Arduino.h>
#include <Wire.h>
#include "DFRobot_Infrared_Positioning.h"	

DFRobot_Infrared_Positioning::DFRobot_Infrared_Positioning()
{
}
DFRobot_Infrared_Positioning::~DFRobot_Infrared_Positioning()
{
}


 
void DFRobot_Infrared_Positioning::initialize(){
	   
	i2cWrite(0x30,0x01); delay(10);
    i2cWrite(0x30,0x08); delay(10);
    i2cWrite(0x06,0x90); delay(10);
    i2cWrite(0x08,0xC0); delay(10);
    i2cWrite(0x1A,0x40); delay(10);
    i2cWrite(0x33,0x33); delay(10);
	delay(100);
	
	
}

void DFRobot_Infrared_Positioning::i2cWrite(uint8_t Reg , uint8_t pdata)
{
  Wire.beginTransmission(ADDRESS);
  Wire.write(Reg);
  Wire.write(pdata);
  Wire.endTransmission();
}

int DFRobot_Infrared_Positioning::ReadInfraredData(uint8_t DataNum) {
	int lxbuf[4], yxbuf[4],k = 0,i;
	
    Wire.beginTransmission(ADDRESS);
    Wire.write(IR_DATA_REGISTER);
    Wire.endTransmission();
    
	Wire.requestFrom(ADDRESS, 16);
	
	for ( i=0;i<16;i++) { 

		InfraredData[i]=0; 
	}
   
    i=0;
	
	 while(Wire.available() && i < 16) { 
        InfraredData[i] = Wire.read();
        i++;
    }
	if(DataNum==0){
		lxbuf[0]=InfraredData[1];
		k = InfraredData[3];
		lxbuf[0] += (k & 0x30) <<4;
		return lxbuf[0];
			
	}
	if(DataNum==1){
		yxbuf[0]=InfraredData[2];
		k = InfraredData[3];
		yxbuf[0] += (k & 0xC0) <<2;
		return yxbuf[0];
			
	}
	if(DataNum==2){
		lxbuf[1]=InfraredData[4];
		k = InfraredData[6];
		lxbuf[1] += (k & 0x30) <<4;
		return lxbuf[1];
			
	}
	if(DataNum==3){
		yxbuf[1]=InfraredData[5];
		k = InfraredData[6];
		yxbuf[1] += (k & 0xC0) <<2;
		return yxbuf[1];
			
	}
	
	if(DataNum==4){
		lxbuf[2]=InfraredData[7];
		k = InfraredData[9];
		lxbuf[2] += (k & 0x30) <<4;
		return lxbuf[2];
			
	}
	if(DataNum==5){
		yxbuf[2]=InfraredData[8];
		k = InfraredData[9];
		yxbuf[2] += (k & 0xC0) <<2;
		return yxbuf[2];
			
	}
	
	if(DataNum==6){
		lxbuf[3]=InfraredData[10];
		k = InfraredData[12];
		lxbuf[3] += (k & 0x30) <<4;
		return lxbuf[3];
			
	}
	if(DataNum==7){
		yxbuf[3]=InfraredData[11];
		k = InfraredData[12];
		yxbuf[3] += (k & 0xC0) <<2;
		return yxbuf[3];
			
	}
}